#include <allegro.h>
#include <math.h>
#include <string>
#include <iostream>
#include <vector>

using namespace std;
int screen_w = 800;
int screen_h = 600;

float robot_x = screen_w/2;
float robot_y = screen_h/2;
float robot_w = 30;
float robot_h = 40;
float robot_angle = 270;
float robot_speed = 0;
int robot_acc = 0;
float xmod = 0;
float ymod = 0;

struct SNeuron {
    // The number of inputs into the neuron
    int m_NumInputs;

    // The weights for each input
    vector<double> m_vecWeight;

    SNeuron(int NumInputs);
};

SNeuron::SNeuron(int NumInputs): m_NumInputs(NumInputs+1) {
    // We need an additional weight for the bias hence the +1
    for (int i=0; i<NumInputs+1; ++i) {
        // Set up the weights with an initial random value
        m_vecWeight.push_back(RandomClamped());
    }
}

struct SNeuronLayer {
    // The number of neurons in this layer
    int m_NumNeurons;

    // The layer of neurons
    vector<SNeuron> m_vecNeurons;

    SNeuronLayer(int NumNeurons, int NumInputsPerNeuron);
};

class CNeuralNet {

private:

    int m_NumInputs;
    int m_NumOutputs;
    int m_NumHiddenLayers;
    int m_NeuronsPerHiddenLyr;

    // Storage for each layer of neurons including the output layer
    vector<SNeuronLayer> m_vecLayers;

public:

    CNeuralNet();

    // Create the neural net
    void CreateNet();

    // Gets the weights from the NN
    vector<double> GetWeights()const;

    // Returns the total number of weights in the net
    int GetNumberOfWeights()const;

    // Replaces the weights with new ones
    void PutWeights(vector<double> &weights);

    // Calculates the outputs from a set of inputs
    vector<double> Update(vector<double> &inputs);

    // Sigmoid response curve
    inline double Sigmoid(double activation, double response);
  
};

vector<double> CNeuralNet::Update(vector<double> &inputs) {

    // Stores the resultant outputs from each layer
    vector<double> outputs;

    int cWeight = 0;

    // First check that we have the correct amount of inputs
    if (inputs.size() != m_NumInputs){
        // Just return an empty vector if incorrect.
        return outputs;
    }

    // For each layer....
    for (int i=0; i<m_NumHiddenLayers + 1; ++i) {

        if ( i > 0 ) {
            inputs = outputs;
        }
        outputs.clear();
        cWeight = 0;

        // For each neuron sum the (inputs * corresponding weights).
        // Throw the total at our sigmoid function to get the output.
        for (int j=0; j<m_vecLayers[i].m_NumNeurons; ++j) {
            double netinput = 0;
            int NumInputs = m_vecLayers[i].m_vecNeurons[j].m_NumInputs;

            // For each weight
            for (int k=0; k<NumInputs - 1; ++k) {
                // Sum the weights x inputs
                netinput += m_vecLayers[i].m_vecNeurons[j].m_vecWeight[k] * inputs[cWeight++];
            }

            // Add in the bias
            netinput += m_vecLayers[i].m_vecNeurons[j].m_vecWeight[NumInputs-1] * CParams::dBias;

            // We can store the outputs from each layer as we generate them.
            // The combined activation is first filtered through the sigmoid
            // function
            outputs.push_back(Sigmoid(netinput, CParams::dActivationResponse));
            cWeight = 0;
        }
    }
    return outputs;
}

struct SGenome {
    vector <double>  vecWeights;
    double dFitness;
    SGenome():dFitness(0){}
    SGenome( vector <double> w, double f): vecWeights(w), dFitness(f){}

    //overload '<' used for sorting
    friend bool operator<(const SGenome& lhs, const SGenome& rhs) {
        return (lhs.dFitness < rhs.dFitness);
    }
};

void robot (BITMAP *buffer, BITMAP *robot) {

    // Controls
    // TODO: Code for acceleration instead of instant speeds
    if (key[KEY_UP])
        robot_speed = 5;
    if (key[KEY_DOWN])
        robot_speed = -5;
    if (key[KEY_LEFT])
        robot_angle -= 4;
    if (key[KEY_RIGHT])
        robot_angle += 4;

    // TODO: Rotate robot sprite 90dcw
    // Some randomised movement

    // Wrap the angle around 0 and 360
    if (robot_angle > 360) {
        robot_angle -= 360;
    }
    if (robot_angle < 0){
        robot_angle = 360 + robot_angle;
    }

    // Make Allegro angle
    float robot_allegro_angle = (robot_angle)/360.0*0xFFFFFF;

    // Applying friction
    if (robot_speed > 0) {
        robot_speed -= 1;
    } else if (robot_speed < 0) {
        robot_speed += 1;
    }

    // Generate actual movement from speed
    if (robot_speed != 0) {
        float robot_angle_radians = robot_angle * M_PI / 180;
        xmod = cos(robot_angle_radians)*robot_speed;
        ymod = sin(robot_angle_radians)*robot_speed;
        robot_x += xmod;
        robot_y += ymod;
    }
    
    int center_x = robot_x - (robot->w/2);
    int center_y = robot_y - (robot->h/2);

    rotate_sprite(buffer, robot, center_x, center_y, robot_allegro_angle);

    rest(25); // Slow stuff down. TODO: Is this a good way to do timing? Figure it out.

}

int main(){
    // Declare stuff
    BITMAP *buffer;
    BITMAP *bmp_robot;
    // INIT Allegro. No allegro stuff before this line.
    allegro_init();

    // Allegro functions here after
    install_keyboard();
    install_mouse();

    // Do color depth and graphics modes here
    int depth = desktop_color_depth();
    if (depth < 32)
       depth = 32;
    set_color_depth(depth);
    set_gfx_mode(GFX_AUTODETECT_WINDOWED, screen_w, screen_h, 0, 0);

    // Get a real cursor
    enable_hardware_cursor();
    show_mouse(screen);

    // Create and load bitmaps
    buffer = create_bitmap(screen_w, screen_h);
    bmp_robot = load_bitmap("robot.bmp", 0);
    
    while(!key[KEY_ESC]){
        clear_to_color(buffer, makecol(255,255,255));
        // Paint stuff to buffer

        robot(buffer, bmp_robot);

        // Stop painting stuff to buffer
        textprintf_right_ex(buffer, font, screen_w - 10, 10, makecol(255,255,255), 1, "rotation: %f", robot_angle); // TODO: Get a string var with status text
        blit(buffer, screen, 0, 0, 0, 0, screen_w, screen_h);
    }

    return 0;

}
END_OF_MAIN();
